#include "Pid.h"
#include "main.h"
#include "tim.h"
#include "Moto.h"
#include "usart.h"
#include "OLED.h"

int moto_cnt = 0;

    // float target_ccr;
    // float previous_ccr;
    // float Kp;
    // float Ki;
    // float Kd;
    // float errork;
    // float errork_1;
    // float errork_2;
PidStruct pidstruct = {
    0,
    0,
    0.5,
    0,
    0,
    0,
    0,
    0
};

uint8_t data_end[4] = {0x00, 0x00, 0x80, 0x7f};

void MotoSpeed_Callback()
{
    pidstruct.previous_ccr = Read_MotoSpeed();
    // int16_t ccr = pidstruct.previous_ccr + cal_pid(&pidstruct);
    // int16_t ccr = cal_pid(&pidstruct);
    // if (ccr < 0)
    // {
    //     ccr = 0;
    // }

    // float data[] = {pidstruct.previous_ccr, (float)ccr, pidstruct.target_ccr};
    OLED_ShowNum(6 * 8, 4, pidstruct.target_ccr, 5, 16, 0);
    // HAL_UART_Transmit(&huart2, (uint8_t*)data, sizeof(data), 10);
    // HAL_UART_Transmit(&huart2, data_end, sizeof(data_end), 10);
    // Switch_Moto_Speed(pidstruct.target_ccr);
    Switch_Moto_Speed(pidstruct.target_ccr);
}

float cal_pid(PidStruct* pidstruct)
{
    pidstruct->errork_2 = pidstruct->errork_1;
    pidstruct->errork_1 = pidstruct->errork;
    pidstruct->errork = pidstruct->target_ccr - pidstruct->previous_ccr;
    // float result = pidstruct->Kp * (pidstruct->errork - pidstruct->errork_1) +
    //     pidstruct->Ki * pidstruct->errork + pidstruct->Kd * (pidstruct->errork - 2 * pidstruct->errork_1 + pidstruct->errork_2);
    float result = pidstruct->Kp * pidstruct->errork + pidstruct->Ki * pidstruct->errork + pidstruct->Kd * (pidstruct->errork - pidstruct->errork_1);
    return result;
}